
#include "sd_mmc_protocol.h"
#include "sd_mmc.h"
#include "hsmci.h"
#include "time_mgr.h"
#include "io_mgr.h"
#include "msc.h"
#include "common_api.h"

#define driver  hsmci
#define SD_MMC_MEM_CNT        SD_MMC_HSMCI_MEM_CNT
#define sd_mmc_is_spi()       false

#define SD_TIME_OVER          3000



#define driver_init                     ATPASTE2(driver, _init)
#define driver_select_device            ATPASTE2(driver, _select_device)
#define driver_deselect_device          ATPASTE2(driver, _deselect_device)
#define driver_get_bus_width            ATPASTE2(driver, _get_bus_width)
#define driver_is_high_speed_capable    ATPASTE2(driver, _is_high_speed_capable)
#define driver_send_clock               ATPASTE2(driver, _send_clock)
#define driver_send_cmd                 ATPASTE2(driver, _send_cmd)
#define driver_get_response             ATPASTE2(driver, _get_response)
#define driver_get_response_128         ATPASTE2(driver, _get_response_128)
#define driver_adtc_start               ATPASTE2(driver, _adtc_start)
#define driver_adtc_stop                ATPASTE2(driver, _send_cmd)
#define driver_read_word                ATPASTE2(driver, _read_word)
#define driver_write_word               ATPASTE2(driver, _write_word)
#define driver_start_read_blocks        ATPASTE2(driver, _start_read_blocks)
#define driver_wait_end_of_read_blocks  ATPASTE2(driver, _wait_end_of_read_blocks)
#define driver_start_write_blocks       ATPASTE2(driver, _start_write_blocks)
#define driver_wait_end_of_write_blocks ATPASTE2(driver, _wait_end_of_write_blocks)

#ifdef SDIO_SUPPORT_ENABLE
#  define IS_SDIO()  (sd_mmc_card->type & CARD_TYPE_SDIO)
#else
#  define IS_SDIO()  false
#endif

#define sd_mmc_is_mci()  (!sd_mmc_is_spi())

//! This SD MMC stack supports only the high voltage
#define SD_MMC_VOLTAGE_SUPPORT \
		(OCR_VDD_27_28 | OCR_VDD_28_29 | \
		OCR_VDD_29_30 | OCR_VDD_30_31 | \
		OCR_VDD_31_32 | OCR_VDD_32_33)

//! SD/MMC card states
enum card_state {
	SD_MMC_CARD_STATE_READY    = 0, //!< Ready to use
	SD_MMC_CARD_STATE_DEBOUNCE = 1, //!< Debounce on going
	SD_MMC_CARD_STATE_INIT     = 2, //!< Initialization on going
	SD_MMC_CARD_STATE_UNUSABLE = 3, //!< Unusable card
	SD_MMC_CARD_STATE_NO_CARD  = 4, //!< No SD/MMC card inserted
};

//! SD/MMC card information structure
struct sd_mmc_card {
	uint32_t clock;            //!< Card access clock
	uint32_t capacity;         //!< Card capacity in KBytes
#if (defined SD_MMC_0_CD_GPIO)
	uint32_t cd_gpio;          //!< Card detect GPIO
#  if (defined SD_MMC_0_WP_GPIO)
	uint32_t wp_gpio;          //!< Card write protection GPIO
#  endif
#endif
	uint16_t rca;              //!< Relative card address
	enum card_state state;     //!< Card state
	card_type_t type;          //!< Card type
	card_version_t version;    //!< Card version
	uint8_t  bus_width;        //!< Number of DATA lin on bus (MCI only)
	uint8_t csd[CSD_REG_BSIZE];//!< CSD register
	uint8_t high_speed;        //!< High speed card (1)
};

//! SD/MMC card list
//! Note: Initialize card detect pin fields if present
static struct sd_mmc_card sd_mmc_cards[SD_MMC_MEM_CNT]
#if (defined SD_MMC_0_CD_GPIO) && (defined SD_MMC_0_WP_GPIO)
 = {
# define SD_MMC_CD_WP(slot, unused) \
	{.cd_gpio = SD_MMC_##slot##_CD_GPIO, \
	.wp_gpio = SD_MMC_##slot##_WP_GPIO},
	MREPEAT(SD_MMC_MEM_CNT, SD_MMC_CD_WP, ~)
# undef SD_MMC_CD_WP
}
#elif (defined SD_MMC_0_CD_GPIO)
 = {
# define SD_MMC_CD(slot, unused) \
	{.cd_gpio = SD_MMC_##slot##_CD_GPIO},
	MREPEAT(SD_MMC_MEM_CNT, SD_MMC_CD, ~)
# undef SD_MMC_CD
}
#endif
;

//! Index of current slot configurated
static uint8_t sd_mmc_slot_sel;
//! Pointer on current slot configurated
static struct sd_mmc_card *sd_mmc_card;
//! Number of block to read or write on the current transfer
static uint16_t sd_mmc_nb_block_to_tranfer = 0;
//! Number of block remaining to read or write on the current transfer
static uint16_t sd_mmc_nb_block_remaining = 0;

//! SD/MMC transfer rate unit codes (10K) list
const uint32_t sd_mmc_trans_units[7] = {
	10, 100, 1000, 10000, 0, 0, 0
};
//! SD transfer multiplier factor codes (1/10) list
const uint32_t sd_trans_multipliers[16] = {
	0, 10, 12, 13, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 70, 80
};
//! MMC transfer multiplier factor codes (1/10) list
const uint32_t mmc_trans_multipliers[16] = {
	0, 10, 12, 13, 15, 20, 26, 30, 35, 40, 45, 52, 55, 60, 70, 80
};

bool sd_initialized = false;

static bool mmc_mci_op_cond(void);
static bool sd_mci_op_cond(uint8_t v2);
static bool sdio_get_max_speed(void);
static bool sdio_cmd52_set_bus_width(void);
static bool sdio_cmd52_set_high_speed(void);
static bool sd_cm6_set_high_speed(void);
static bool mmc_cmd6_set_bus_width(uint8_t bus_width);
static bool mmc_cmd6_set_high_speed(void);
static bool sd_cmd8(uint8_t * v2);
static bool mmc_cmd8(uint8_t *b_authorize_high_speed);
static bool sd_mmc_cmd9_mci(void);
static void mmc_decode_csd(void);
static void sd_decode_csd(void);
static bool sd_mmc_cmd13(void);
#ifdef SDIO_SUPPORT_ENABLE
static bool sdio_cmd52(uint8_t rw_flag, uint8_t func_nb,
		uint32_t reg_addr, uint8_t rd_after_wr, uint8_t *io_data);
static bool sdio_cmd53(uint8_t rw_flag, uint8_t func_nb, uint32_t reg_addr,
		uint8_t inc_addr, uint32_t size, bool access_block);
#endif // SDIO_SUPPORT_ENABLE
static bool sd_acmd6(void);
static bool sd_acmd51(void);

static sd_mmc_err_t sd_mmc_select_slot(uint8_t slot);
static void sd_mmc_configure_slot(void);
static void sd_mmc_deselect_slot(void);
static bool sd_mmc_mci_card_init(void);
static bool sd_mmc_mci_install_mmc(void);

#define SD_MMC_DEBOUNCE_TIMEOUT   1000 // Unit ms

static bool mmc_mci_op_cond(void)
{
	uint32_t retry, resp;

	//Timeout 1s = 400KHz / ((6+6)*8) cylces = 4200 retry
	//6 = cmd byte size
	//6 = response byte size

	retry = 4200;
	do {
		if (!driver_send_cmd(MMC_MCI_CMD1_SEND_OP_COND,
				SD_MMC_VOLTAGE_SUPPORT | OCR_ACCESS_MODE_SECTOR)) {

			return false;
		}
		// Check busy flag
		resp = driver_get_response();
		if (resp & OCR_POWER_UP_BUSY) {
			// Check OCR value
			if ((resp & OCR_ACCESS_MODE_MASK)
					== OCR_ACCESS_MODE_SECTOR) {
				sd_mmc_card->type |= CARD_TYPE_HC;
			}
			break;
		}
		if (retry-- == 0) {

			return false;
		}
	} while (1);
	return true;
}

static bool sd_mci_op_cond(uint8_t v2)
{
	uint32_t arg, retry, resp;

	 //Timeout 1s = 400KHz / ((6+6+6+6)*8) cylces = 2100 retry
	 //6 = cmd byte size
	 //6 = response byte size
	 //6 = cmd byte size
	 //6 = response byte size

	retry = 2100;
	do {
		// CMD55 - Indicate to the card that the next command is an
		// application specific command rather than a standard command.
		if (!driver_send_cmd(SDMMC_CMD55_APP_CMD, 0)) {
			return false;
		}

		// (ACMD41) Sends host OCR register
		arg = SD_MMC_VOLTAGE_SUPPORT;
		if (v2) {
			arg |= SD_ACMD41_HCS;
		}
		// Check response
		if (!driver_send_cmd(SD_MCI_ACMD41_SD_SEND_OP_COND, arg)) {
			return false;
		}
		resp = driver_get_response();
		if (resp & OCR_POWER_UP_BUSY) {
			// Card is ready
			if ((resp & OCR_CCS) != 0) {
				sd_mmc_card->type |= CARD_TYPE_HC;
			}
			break;
		}
		if (retry-- == 0) {
			return false;
		}
	} while (1);
	return true;
}

#ifdef SDIO_SUPPORT_ENABLE
static bool sdio_op_cond(void)
{
	uint32_t resp;

	// CMD5 - SDIO send operation condition (OCR) command.
	if (!driver_send_cmd(SDIO_CMD5_SEND_OP_COND, 0)) {
		return true; // No error but card type not updated
	}
	resp = driver_get_response();
	if ((resp & OCR_SDIO_NF) == 0) {
		return true; // No error but card type not updated
	}

	//Wait card ready
	//Timeout 1s = 400KHz / ((6+4)*8) cylces = 5000 retry
	//6 = cmd byte size
	//4(SPI) 6(MCI) = response byte size

	uint32_t cmd5_retry = 5000;
	while (1) {
		// CMD5 - SDIO send operation condition (OCR) command.
		if (!driver_send_cmd(SDIO_CMD5_SEND_OP_COND,
				resp & SD_MMC_VOLTAGE_SUPPORT)) {
			return false;
		}
		resp = driver_get_response();
		if ((resp & OCR_POWER_UP_BUSY) == OCR_POWER_UP_BUSY) {
			break;
		}
		if (cmd5_retry-- == 0) {
			return false;
		}
	}
	// Update card type at the end of busy
	if ((resp & OCR_SDIO_MP) > 0) {
		sd_mmc_card->type = CARD_TYPE_SD_COMBO;
	} else {
		sd_mmc_card->type = CARD_TYPE_SDIO;
	}
	return true; // No error and card type updated with SDIO type
}

static bool sdio_get_max_speed(void)
{
	uint32_t addr, addr_cis;
	uint8_t buf[6];
 	uint32_t unit;
	uint32_t mul;
	uint8_t tplfe_max_tran_speed;

	// Read CIS area address in CCCR area
	addr_cis = 0; // Init all bytes, because the next function fill 3 bytes only
	if (!sdio_cmd53(SDIO_CMD53_READ_FLAG, SDIO_CIA, SDIO_CCCR_CIS_PTR,
			1, 3, true)) {
		return false;
	}
	if (!driver_start_read_blocks((uint8_t *)&addr_cis, 1)) {
		return false;
	}
	if (!driver_wait_end_of_read_blocks()) {
		return false;
	}
	addr_cis = le32_to_cpu(addr_cis);

	// Search Fun0 tuple in the CIA area
	addr = addr_cis;
	while (1) {
		// Read a sample of CIA area
		if (!sdio_cmd53(SDIO_CMD53_READ_FLAG, SDIO_CIA, addr, 1, 3, true)) {
			return false;
		}
		if (!driver_start_read_blocks(buf, 1)) {
			return false;
		}
		if (!driver_wait_end_of_read_blocks()) {
			return false;
		}
		if (buf[0] == SDIO_CISTPL_END) {
			return false; // Tuple error
		}
		if (buf[0] == SDIO_CISTPL_FUNCE && buf[2] == 0x00) {
			break; // Fun0 tuple found
		}
		if (buf[1] == 0) {
			return false; // Tuple error
		}

		// Next address
		addr += (buf[1] + 2);
		if (addr > (addr_cis + 256)) {
			return false; // Outoff CIS area
		}
	}

	// Read all Fun0 tuple fields: fn0_blk_siz & max_tran_speed
	if (!sdio_cmd53(SDIO_CMD53_READ_FLAG, SDIO_CIA, addr, 1, 6, true)) {
		return false;
	}
	if (!driver_start_read_blocks(buf, 1)) {
		return false;
	}
	if (!driver_wait_end_of_read_blocks()) {
		return false;
	}
	tplfe_max_tran_speed = buf[5];
	if (tplfe_max_tran_speed > 0x32) {
		tplfe_max_tran_speed = 0x32; // 25Mhz
	}

	// Decode transfer speed in Hz.
	unit = sd_mmc_trans_units[tplfe_max_tran_speed & 0x7];
	mul = sd_trans_multipliers[(tplfe_max_tran_speed >> 3) & 0xF];
	sd_mmc_card->clock = unit * mul * 1000;

	return true;
}

static bool sdio_cmd52_set_bus_width(void)
{
	uint8_t u8_value;

	// Check 4bit support in 4BLS of "Card Capability" register
	if (!sdio_cmd52(SDIO_CMD52_READ_FLAG, SDIO_CIA, SDIO_CCCR_CAP,
			0, &u8_value)) {
		return false;
	}
	if ((u8_value & SDIO_CAP_4BLS) != SDIO_CAP_4BLS) {
		// No supported, it is not a protocol error
		return true;
	}
	// HS mode possible, then enable
	u8_value = SDIO_BUSWIDTH_4B;
	if (!sdio_cmd52(SDIO_CMD52_WRITE_FLAG, SDIO_CIA, SDIO_CCCR_BUS_CTRL,
			1, &u8_value)) {
		return false;
	}
	sd_mmc_card->bus_width = 4;
	return true;
}

static bool sdio_cmd52_set_high_speed(void)
{
	uint8_t u8_value;

	// Check CIA.HS
	if (!sdio_cmd52(SDIO_CMD52_READ_FLAG, SDIO_CIA, SDIO_CCCR_HS, 0, &u8_value)) {
		return false;
	}
	if ((u8_value & SDIO_SHS) != SDIO_SHS) {
		// No supported, it is not a protocol error
		return true;
	}
	// HS mode possible, then enable
	u8_value = SDIO_EHS;
	if (!sdio_cmd52(SDIO_CMD52_WRITE_FLAG, SDIO_CIA, SDIO_CCCR_HS,
			1, &u8_value)) {
		return false;
	}
	sd_mmc_card->high_speed = 1;
	sd_mmc_card->clock *= 2;
	return true;
}

#else
static bool sdio_get_max_speed(void)
{
	return false;
}
static bool sdio_cmd52_set_bus_width(void)
{
	return false;
}
static bool sdio_cmd52_set_high_speed(void)
{
	return false;
}
#endif // SDIO_SUPPORT_ENABLE

static bool sd_cm6_set_high_speed(void)
{
	uint8_t switch_status[SD_SW_STATUS_BSIZE];

	if (!driver_adtc_start(SD_CMD6_SWITCH_FUNC,
			SD_CMD6_MODE_SWITCH
			| SD_CMD6_GRP6_NO_INFLUENCE
			| SD_CMD6_GRP5_NO_INFLUENCE
			| SD_CMD6_GRP4_NO_INFLUENCE
			| SD_CMD6_GRP3_NO_INFLUENCE
			| SD_CMD6_GRP2_DEFAULT
			| SD_CMD6_GRP1_HIGH_SPEED,
			SD_SW_STATUS_BSIZE, 1, true)) {
		return false;
	}
	if (!driver_start_read_blocks(switch_status, 1)) {
		return false;
	}
	if (!driver_wait_end_of_read_blocks()) {
		return false;
	}

	if (driver_get_response() & CARD_STATUS_SWITCH_ERROR) {
		return false;
	}
	if (SD_SW_STATUS_FUN_GRP1_RC(switch_status)
			== SD_SW_STATUS_FUN_GRP_RC_ERROR) {
		// No supported, it is not a protocol error
		return true;
	}
	if (SD_SW_STATUS_FUN_GRP1_BUSY(switch_status)) {
		return false;
	}
	// CMD6 function switching period is within 8 clocks
	// after the end bit of status data.
	driver_send_clock();
	sd_mmc_card->high_speed = 1;
	sd_mmc_card->clock *= 2;
	return true;
}

static bool mmc_cmd6_set_bus_width(uint8_t bus_width)
{
	uint32_t arg;

	switch (bus_width) {
	case 8:
		arg = MMC_CMD6_ACCESS_SET_BITS
				| MMC_CMD6_INDEX_BUS_WIDTH
				| MMC_CMD6_VALUE_BUS_WIDTH_8BIT;
		break;
	case 4:
		arg = MMC_CMD6_ACCESS_SET_BITS
				| MMC_CMD6_INDEX_BUS_WIDTH
				| MMC_CMD6_VALUE_BUS_WIDTH_4BIT;
		break;
	default:
		arg = MMC_CMD6_ACCESS_SET_BITS
				| MMC_CMD6_INDEX_BUS_WIDTH
				| MMC_CMD6_VALUE_BUS_WIDTH_1BIT;
		break;
	}
	if (!driver_send_cmd(MMC_CMD6_SWITCH, arg)) {
		return false;
	}
	if (driver_get_response() & CARD_STATUS_SWITCH_ERROR) {
		// No supported, it is not a protocol error
		return false;
	}
	sd_mmc_card->bus_width = bus_width;
	return true;
}

static bool mmc_cmd6_set_high_speed(void)
{
	if (!driver_send_cmd(MMC_CMD6_SWITCH,
			MMC_CMD6_ACCESS_WRITE_BYTE
			| MMC_CMD6_INDEX_HS_TIMING
			| MMC_CMD6_VALUE_HS_TIMING_ENABLE)) {
		return false;
	}
	if (driver_get_response() & CARD_STATUS_SWITCH_ERROR) {
		// No supported, it is not a protocol error
		//sd_mmc_debug("%s: CMD6 CARD_STATUS_SWITCH_ERROR\n\r", __func__);
		return false;
	}
	sd_mmc_card->high_speed = 1;
	sd_mmc_card->clock = 52000000lu;
	return true;
}

static bool sd_cmd8(uint8_t * v2)
{
	uint32_t resp;

	*v2 = 0;
	// Test for SD version 2
	if (!driver_send_cmd(SD_CMD8_SEND_IF_COND,
			SD_CMD8_PATTERN | SD_CMD8_HIGH_VOLTAGE)) {
		return true; // It is not a V2
	}
	// Check R7 response
	resp = driver_get_response();
	if (resp == 0xFFFFFFFF) {
		// No compliance R7 value
		return true; // It is not a V2
	}
	if ((resp & (SD_CMD8_MASK_PATTERN | SD_CMD8_MASK_VOLTAGE))
				!= (SD_CMD8_PATTERN | SD_CMD8_HIGH_VOLTAGE)) {
		return false;
	}

	*v2 = 1;
	return true;
}

static bool mmc_cmd8(uint8_t *b_authorize_high_speed)
{
	uint16_t i;
	uint32_t ext_csd;
	uint32_t sec_count;

	if (!driver_adtc_start(MMC_CMD8_SEND_EXT_CSD, 0,
			EXT_CSD_BSIZE, 1, false)) {
		return false;
	}
	//** Read and decode Extended Extended CSD
	// Note: The read access is done in byte to avoid a buffer
	// of EXT_CSD_BSIZE Byte in stack.

	// Read card type
	for (i = 0; i < (EXT_CSD_CARD_TYPE_INDEX + 4) / 4; i++) {
		if (!driver_read_word(&ext_csd)) {
			return false;
		}
	}
	*b_authorize_high_speed = (ext_csd >> ((EXT_CSD_CARD_TYPE_INDEX % 4) * 8))
			& MMC_CTYPE_52MHZ;

	if (MMC_CSD_C_SIZE(sd_mmc_card->csd) == 0xFFF) {
		// For high capacity SD/MMC card,
		// memory capacity = SEC_COUNT * 512 byte
		for (; i <(EXT_CSD_SEC_COUNT_INDEX + 4) / 4; i++) {
			if (!driver_read_word(&sec_count)) {
				return false;
			}
		}
		sd_mmc_card->capacity = sec_count / 2;
	}
	for (; i < EXT_CSD_BSIZE / 4; i++) {
		if (!driver_read_word(&sec_count)) {
			return false;
		}
	}
	return true;
}

static bool sd_mmc_cmd9_mci(void)
{
	if (!driver_send_cmd(SDMMC_MCI_CMD9_SEND_CSD, (uint32_t)sd_mmc_card->rca << 16)) {
		return false;
	}
	driver_get_response_128(sd_mmc_card->csd);
	return true;
}

static void mmc_decode_csd(void)
{
 	uint32_t unit;
	uint32_t mul;
	uint32_t tran_speed;

	// Get MMC System Specification version supported by the card
	switch (MMC_CSD_SPEC_VERS(sd_mmc_card->csd)) {
	default:
	case 0:
		sd_mmc_card->version = CARD_VER_MMC_1_2;
		break;

	case 1:
		sd_mmc_card->version = CARD_VER_MMC_1_4;
		break;

	case 2:
		sd_mmc_card->version = CARD_VER_MMC_2_2;
		break;

	case 3:
		sd_mmc_card->version = CARD_VER_MMC_3;
		break;

	case 4:
		sd_mmc_card->version = CARD_VER_MMC_4;
		break;
	}

	// Get MMC memory max transfer speed in Hz.
	tran_speed = CSD_TRAN_SPEED(sd_mmc_card->csd);
	unit = sd_mmc_trans_units[tran_speed & 0x7];
	mul = mmc_trans_multipliers[(tran_speed >> 3) & 0xF];
	sd_mmc_card->clock = unit * mul * 1000;

	if (MMC_CSD_C_SIZE(sd_mmc_card->csd) != 0xFFF) {
		uint32_t blocknr = ((MMC_CSD_C_SIZE(sd_mmc_card->csd) + 1) *
			(1 << (MMC_CSD_C_SIZE_MULT(sd_mmc_card->csd) + 2)));
		sd_mmc_card->capacity = blocknr *
			(1 << MMC_CSD_READ_BL_LEN(sd_mmc_card->csd)) / 1024;
	}
}

static void sd_decode_csd(void)
{
 	uint32_t unit;
	uint32_t mul;
	uint32_t tran_speed;

	// Get SD memory maximum transfer speed in Hz.
	tran_speed = CSD_TRAN_SPEED(sd_mmc_card->csd);
	unit = sd_mmc_trans_units[tran_speed & 0x7];
	mul = sd_trans_multipliers[(tran_speed >> 3) & 0xF];
	sd_mmc_card->clock = unit * mul * 1000;

	if (CSD_STRUCTURE_VERSION(sd_mmc_card->csd) >= SD_CSD_VER_2_0) {
		sd_mmc_card->capacity =
				(SD_CSD_2_0_C_SIZE(sd_mmc_card->csd) + 1)
				* 512;
	} else {
		uint32_t blocknr = ((SD_CSD_1_0_C_SIZE(sd_mmc_card->csd) + 1) *
				(1 << (SD_CSD_1_0_C_SIZE_MULT(sd_mmc_card->csd) + 2)));
		sd_mmc_card->capacity = blocknr *
				(1 << SD_CSD_1_0_READ_BL_LEN(sd_mmc_card->csd))
				/ 1024;
	}
        
        sd_mmc_card->capacity -= SD_BLK_OFFSET_ADDR/2;
}

static bool sd_mmc_cmd13(void)
{
	uint32_t nec_timeout;

	nec_timeout = 200000;
	do {
		if (sd_mmc_is_spi()) {
			if (!driver_send_cmd(SDMMC_SPI_CMD13_SEND_STATUS, 0)) {
				return false;
			}
			// Check busy flag
			if (!(driver_get_response() & 0xFF)) {
				break;
			}
		} else {
			if (!driver_send_cmd(SDMMC_MCI_CMD13_SEND_STATUS,
					(uint32_t)sd_mmc_card->rca << 16)) {
				return false;
			}
			// Check busy flag
			if (driver_get_response() & CARD_STATUS_READY_FOR_DATA) {
				break;
			}
		}
		if (nec_timeout-- == 0) {
			return false;
		}
	} while (1);

	return true;
}

#ifdef SDIO_SUPPORT_ENABLE
static bool sdio_cmd52(uint8_t rw_flag, uint8_t func_nb,
		uint32_t reg_addr, uint8_t rd_after_wr, uint8_t *io_data)
{
	if (!driver_send_cmd(SDIO_CMD52_IO_RW_DIRECT,
		((uint32_t)*io_data << SDIO_CMD52_WR_DATA)
		| ((uint32_t)rw_flag << SDIO_CMD52_RW_FLAG)
		| ((uint32_t)func_nb << SDIO_CMD52_FUNCTION_NUM)
		| ((uint32_t)rd_after_wr << SDIO_CMD52_RAW_FLAG)
		| ((uint32_t)reg_addr << SDIO_CMD52_REG_ADRR))) {
		return false;
	}
	*io_data = driver_get_response() & 0xFF;
	return true;
}

static bool sdio_cmd53(uint8_t rw_flag, uint8_t func_nb, uint32_t reg_addr,
		uint8_t inc_addr, uint32_t size, bool access_block)
{
	return driver_adtc_start((rw_flag == SDIO_CMD53_READ_FLAG)?
			SDIO_CMD53_IO_R_BYTE_EXTENDED :
			SDIO_CMD53_IO_W_BYTE_EXTENDED,
			((size % 512) << SDIO_CMD53_COUNT)
			| ((uint32_t)reg_addr << SDIO_CMD53_REG_ADDR)
			| ((uint32_t)inc_addr << SDIO_CMD53_OP_CODE)
			| ((uint32_t)0 << SDIO_CMD53_BLOCK_MODE)
			| ((uint32_t)func_nb << SDIO_CMD53_FUNCTION_NUM)
			| ((uint32_t)rw_flag << SDIO_CMD53_RW_FLAG),
			size, 1, access_block);
}
#endif // SDIO_SUPPORT_ENABLE

static bool sd_acmd6(void)
{
	// CMD55 - Indicate to the card that the next command is an
	// application specific command rather than a standard command.
	if (!driver_send_cmd(SDMMC_CMD55_APP_CMD, (uint32_t)sd_mmc_card->rca << 16)) {
		return false;
	}
	// 10b = 4 bits bus
	if (!driver_send_cmd(SD_ACMD6_SET_BUS_WIDTH, 0x2)) {
		return false;
	}
	sd_mmc_card->bus_width = 4;
	return true;
}

static bool sd_acmd51(void)
{
	uint8_t scr[SD_SCR_REG_BSIZE];

	// CMD55 - Indicate to the card that the next command is an
	// application specific command rather than a standard command.
	if (!driver_send_cmd(SDMMC_CMD55_APP_CMD, (uint32_t)sd_mmc_card->rca << 16)) {
		return false;
	}
	if (!driver_adtc_start(SD_ACMD51_SEND_SCR, 0,
			SD_SCR_REG_BSIZE, 1, true)) {
		return false;
	}
	if (!driver_start_read_blocks(scr, 1)) {
		return false;
	}
	if (!driver_wait_end_of_read_blocks()) {
		return false;
	}

	// Get SD Memory Card - Spec. Version
	switch (SD_SCR_SD_SPEC(scr)) {
	case SD_SCR_SD_SPEC_1_0_01:
		sd_mmc_card->version = CARD_VER_SD_1_0;
		break;

	case SD_SCR_SD_SPEC_1_10:
		sd_mmc_card->version = CARD_VER_SD_1_10;
		break;

	case SD_SCR_SD_SPEC_2_00:
		if (SD_SCR_SD_SPEC3(scr) == SD_SCR_SD_SPEC_3_00) {
			sd_mmc_card->version = CARD_VER_SD_3_0;
		} else {
			sd_mmc_card->version = CARD_VER_SD_2_0;
		}
		break;

	default:
		sd_mmc_card->version = CARD_VER_SD_1_0;
		break;
	}
	return true;
}

static sd_mmc_err_t sd_mmc_select_slot(uint8_t slot)
{
	if (slot >= SD_MMC_MEM_CNT) {
		return SD_MMC_ERR_SLOT;
	}

#if (defined SD_MMC_0_CD_GPIO)
	//! Card Detect pins
	if (ioport_get_pin_level(sd_mmc_cards[slot].cd_gpio)
			!= SD_MMC_0_CD_DETECT_VALUE) {
		if (sd_mmc_cards[slot].state == SD_MMC_CARD_STATE_DEBOUNCE) {
			SD_MMC_STOP_TIMEOUT();
		}
		sd_mmc_cards[slot].state = SD_MMC_CARD_STATE_NO_CARD;
		return SD_MMC_ERR_NO_CARD;
	}
	if (sd_mmc_cards[slot].state == SD_MMC_CARD_STATE_NO_CARD) {
		// A card plug on going, but this is not initialized
		sd_mmc_cards[slot].state = SD_MMC_CARD_STATE_DEBOUNCE;
		// Debounce + Power On Setup
		SD_MMC_START_TIMEOUT();
		return SD_MMC_ERR_NO_CARD;
	}
	if (sd_mmc_cards[slot].state == SD_MMC_CARD_STATE_DEBOUNCE) {
		if (!SD_MMC_IS_TIMEOUT()) {
			// Debounce on going
			return SD_MMC_ERR_NO_CARD;
		}
		// Card is not initialized
		sd_mmc_cards[slot].state = SD_MMC_CARD_STATE_INIT;
		// Set 1-bit bus width and low clock for initialization
		sd_mmc_cards[slot].clock = SDMMC_CLOCK_INIT;
		sd_mmc_cards[slot].bus_width = 1;
		sd_mmc_cards[slot].high_speed = 0;
	}
	if (sd_mmc_cards[slot].state == SD_MMC_CARD_STATE_UNUSABLE) {
		return SD_MMC_ERR_UNUSABLE;
	}
#else
	// No pin card detection, then always try to install it
	if ((sd_mmc_cards[slot].state == SD_MMC_CARD_STATE_NO_CARD)
			|| (sd_mmc_cards[slot].state == SD_MMC_CARD_STATE_UNUSABLE)) {
		// Card is not initialized
		sd_mmc_cards[slot].state = SD_MMC_CARD_STATE_INIT;
		// Set 1-bit bus width and low clock for initialization
		sd_mmc_cards[slot].clock = SDMMC_CLOCK_INIT;
		sd_mmc_cards[slot].bus_width = 1;
		sd_mmc_cards[slot].high_speed = 0;
	}
#endif

	// Initialize interface
	sd_mmc_slot_sel = slot;
	sd_mmc_card = &sd_mmc_cards[slot];
	sd_mmc_configure_slot();
	return (sd_mmc_cards[slot].state == SD_MMC_CARD_STATE_INIT) ?
			SD_MMC_INIT_ONGOING : SD_MMC_OK;
}

static void sd_mmc_configure_slot(void)
{
	driver_select_device(sd_mmc_slot_sel, sd_mmc_card->clock,
			sd_mmc_card->bus_width, sd_mmc_card->high_speed);
}

static void sd_mmc_deselect_slot(void)
{
	if (sd_mmc_slot_sel < SD_MMC_MEM_CNT) {
		driver_deselect_device(sd_mmc_slot_sel);
	}
}

static bool sd_mmc_mci_card_init(void)
{
	uint8_t v2 = 0;

	// In first, try to install SD/SDIO card
	sd_mmc_card->type = CARD_TYPE_SD;
	sd_mmc_card->version = CARD_VER_UNKNOWN;
	sd_mmc_card->rca = 0;
	//sd_mmc_debug("Start SD card install\n\r");

	// Card need of 74 cycles clock minimum to start
	driver_send_clock();

	// CMD0 - Reset all cards to idle state.
	if (!driver_send_cmd(SDMMC_MCI_CMD0_GO_IDLE_STATE, 0)) {
		return false;
	}
	if (!sd_cmd8(&v2)) {
		return false;
	}
	// Try to get the SDIO card's operating condition
	if (sd_mmc_card->type & CARD_TYPE_SD) {
		// Try to get the SD card's operating condition
		if (!sd_mci_op_cond(v2)) {
			// It is not a SD card
			sd_mmc_card->type = CARD_TYPE_MMC;
			return sd_mmc_mci_install_mmc();
		}
	}

	if (sd_mmc_card->type & CARD_TYPE_SD) {
		// SD MEMORY, Put the Card in Identify Mode
		// Note: The CID is not used in this stack
		if (!driver_send_cmd(SDMMC_CMD2_ALL_SEND_CID, 0)) {
			return false;
		}
                driver_get_response_128(sd_card_sn);
	}
	// Ask the card to publish a new relative address (RCA).
	if (!driver_send_cmd(SD_CMD3_SEND_RELATIVE_ADDR, 0)) {
		return false;
	}
	sd_mmc_card->rca = (driver_get_response() >> 16) & 0xFFFF;

	// SD MEMORY, Get the Card-Specific Data
	if (sd_mmc_card->type & CARD_TYPE_SD) {
		if (!sd_mmc_cmd9_mci()) {
			return false;
		}
		sd_decode_csd();
	}
	// Select the and put it into Transfer Mode
	if (!driver_send_cmd(SDMMC_CMD7_SELECT_CARD_CMD,
			(uint32_t)sd_mmc_card->rca << 16)) {
		return false;
	}
	// SD MEMORY, Read the SCR to get card version
	if (sd_mmc_card->type & CARD_TYPE_SD) {
		if (!sd_acmd51()) {
			return false;
		}
	}
	if (IS_SDIO()) {
		if (!sdio_get_max_speed()) {
			return false;
		}
	}
	if ((4 <= driver_get_bus_width(sd_mmc_slot_sel))) {
		// TRY to enable 4-bit mode
		if (IS_SDIO()) {
			if (!sdio_cmd52_set_bus_width()) {
				return false;
			}
		}
		if (sd_mmc_card->type & CARD_TYPE_SD) {
			if (!sd_acmd6()) {
				return false;
			}
		}
		// Switch to selected bus mode
		sd_mmc_configure_slot();
	}
	if (driver_is_high_speed_capable()) {
		// TRY to enable High-Speed Mode
		if (IS_SDIO()) {
			if (!sdio_cmd52_set_high_speed()) {
				return false;
			}
		}
		if (sd_mmc_card->type & CARD_TYPE_SD) {
			if (sd_mmc_card->version > CARD_VER_SD_1_0) {
				if (!sd_cm6_set_high_speed()) {
					return false;
				}
			}
		}
		// Valid new configuration
		sd_mmc_configure_slot();
	}
	// SD MEMORY, Set default block size
	if (sd_mmc_card->type & CARD_TYPE_SD) {
		if (!driver_send_cmd(SDMMC_CMD16_SET_BLOCKLEN, SD_MMC_BLOCK_SIZE)) {
			return false;
		}
	}
	return true;
}

static bool sd_mmc_mci_install_mmc(void)
{
	uint8_t b_authorize_high_speed;

	// CMD0 - Reset all cards to idle state.
	if (!driver_send_cmd(SDMMC_MCI_CMD0_GO_IDLE_STATE, 0)) {
		return false;
	}

	if (!mmc_mci_op_cond()) {
		return false;
	}

	// Put the Card in Identify Mode
	// Note: The CID is not used in this stack
	if (!driver_send_cmd(SDMMC_CMD2_ALL_SEND_CID, 0)) {
		return false;
	}
	// Assign relative address to the card.
	sd_mmc_card->rca = 1;
	if (!driver_send_cmd(MMC_CMD3_SET_RELATIVE_ADDR,
			(uint32_t)sd_mmc_card->rca << 16)) {
		return false;
	}
	// Get the Card-Specific Data
	if (!sd_mmc_cmd9_mci()) {
		return false;
	}
	mmc_decode_csd();
	// Select the and put it into Transfer Mode
	if (!driver_send_cmd(SDMMC_CMD7_SELECT_CARD_CMD,
			(uint32_t)sd_mmc_card->rca << 16)) {
		return false;
	}
	if (sd_mmc_card->version >= CARD_VER_MMC_4) {
		// For MMC 4.0 Higher version
		// Get EXT_CSD
		if (!mmc_cmd8(&b_authorize_high_speed)) {
			return false;
		}
		if (4 <= driver_get_bus_width(sd_mmc_slot_sel)) {
			// Enable more bus width
			if (!mmc_cmd6_set_bus_width(driver_get_bus_width(sd_mmc_slot_sel))) {
				return false;
			}
			// Reinitialize the slot with the bus width
			sd_mmc_configure_slot();
		}
		if (driver_is_high_speed_capable() && b_authorize_high_speed) {
			// Enable HS
			if (!mmc_cmd6_set_high_speed()) {
				return false;
			}
			// Reinitialize the slot with the new speed
			sd_mmc_configure_slot();
		}
	} else {
		// Reinitialize the slot with the new speed
		sd_mmc_configure_slot();
	}

	uint8_t retry = 10;
	while (retry--) {
		// Retry is a WORKAROUND for no compliance card (Atmel Internal ref. MMC19):
		// These cards seem not ready immediatly
		// after the end of busy of mmc_cmd6_set_high_speed()

		// Set default block size
		if (driver_send_cmd(SDMMC_CMD16_SET_BLOCKLEN, SD_MMC_BLOCK_SIZE)) {
			return true;
		}
	}
	return false;
}

//-------------------------------------------------------------------
//--------------------- PUBLIC FUNCTIONS ----------------------------

void sd_mmc_init(void)
{
        //uint32_t  sd_start_time;
          
        sd_mmc_err_t sd_mmc_err;
        
        sd_initialized = false;
        
	for (uint8_t slot = 0; slot < SD_MMC_MEM_CNT; slot++) {
		sd_mmc_cards[slot].state = SD_MMC_CARD_STATE_NO_CARD;
	}
	sd_mmc_slot_sel = 0xFF; // No slot configurated
	driver_init();
        
        //wait_ms(300);
        
        //sd_start_time = get_system_time();
        start_temp_timer();
        
        do {
        sd_mmc_err = sd_mmc_check(0);
        if ((SD_MMC_ERR_NO_CARD != sd_mmc_err)
                && (SD_MMC_INIT_ONGOING != sd_mmc_err)
                && (SD_MMC_OK != sd_mmc_err)) 
        {
            while (SD_MMC_ERR_NO_CARD != sd_mmc_check(0)) 
            {
                if(Is_temp_time_over(SD_TIME_OVER))
                {
                    stop_temp_timer();
                    return;
                }
            }
        }
        if(Is_temp_time_over(SD_TIME_OVER))
        {
            stop_temp_timer();
            return;
        }
        /*if((get_system_time() - sd_start_time) > SD_TIME_OVER)
        {
            return;
        }*/
        }while (SD_MMC_OK != sd_mmc_err);
        
        stop_temp_timer();
        
        sd_initialized = true;
}

uint8_t sd_mmc_nb_slot(void)
{
	return SD_MMC_MEM_CNT;
}

sd_mmc_err_t sd_mmc_check(uint8_t slot)
{
	sd_mmc_err_t sd_mmc_err;

	sd_mmc_err = sd_mmc_select_slot(slot);
	if (sd_mmc_err != SD_MMC_INIT_ONGOING) {
		sd_mmc_deselect_slot();
		return sd_mmc_err;
	}

	// Initialization of the card requested
	if (sd_mmc_mci_card_init()) {
		sd_mmc_card->state = SD_MMC_CARD_STATE_READY;
		sd_mmc_deselect_slot();
		// To notify that the card has been just initialized
		// It is necessary for USB Device MSC
		return SD_MMC_INIT_ONGOING;
	}
	sd_mmc_card->state = SD_MMC_CARD_STATE_UNUSABLE;
	sd_mmc_deselect_slot();
	return SD_MMC_ERR_UNUSABLE;
}

card_type_t sd_mmc_get_type(uint8_t slot)
{
	if (SD_MMC_OK != sd_mmc_select_slot(slot)) {
		return CARD_TYPE_UNKNOWN;
	}
	sd_mmc_deselect_slot();
	return sd_mmc_card->type;
}

card_version_t sd_mmc_get_version(uint8_t slot)
{
	if (SD_MMC_OK != sd_mmc_select_slot(slot)) {
		return CARD_VER_UNKNOWN;
	}
	sd_mmc_deselect_slot();
	return sd_mmc_card->version;
}

uint32_t sd_mmc_get_capacity(uint8_t slot)
{
	if (SD_MMC_OK != sd_mmc_select_slot(slot)) {
		return 0;
	}
	sd_mmc_deselect_slot();
	return sd_mmc_card->capacity;
}

bool sd_mmc_is_write_protected(uint8_t slot)
{
	UNUSED(slot);
#if (defined SD_MMC_0_WP_GPIO)
	//! Card Detect pins
	if (ioport_get_pin_level(sd_mmc_cards[slot].wp_gpio)
			== SD_MMC_0_WP_DETECT_VALUE) {
		return true;
	}
#endif
	return false;
}

sd_mmc_err_t sd_mmc_init_read_blocks(uint8_t slot, uint32_t start,
		uint16_t nb_block)
{
	sd_mmc_err_t sd_mmc_err;
	uint32_t cmd, arg, resp;

	sd_mmc_err = sd_mmc_select_slot(slot);
	if (sd_mmc_err != SD_MMC_OK) {
		return sd_mmc_err;
	}

	// Wait for data ready status
	if (!sd_mmc_cmd13()) {
		sd_mmc_deselect_slot();
		return SD_MMC_ERR_COMM;
	}

	if (nb_block > 1) {
		cmd = SDMMC_CMD18_READ_MULTIPLE_BLOCK;
	} else {
		cmd = SDMMC_CMD17_READ_SINGLE_BLOCK;
	}

	if (sd_mmc_card->type & CARD_TYPE_HC) {
		arg = start;
	} else {
		arg = (start * SD_MMC_BLOCK_SIZE);
	}

	if (!driver_adtc_start(cmd, arg, SD_MMC_BLOCK_SIZE, nb_block, true)) {
		sd_mmc_deselect_slot();
		return SD_MMC_ERR_COMM;
	}
	// Check response
	if (sd_mmc_is_mci()) {
		resp = driver_get_response();
		if (resp & CARD_STATUS_ERR_RD_WR) {
			sd_mmc_deselect_slot();
			return SD_MMC_ERR_COMM;
		}
	}
	sd_mmc_nb_block_remaining = nb_block;
	sd_mmc_nb_block_to_tranfer = nb_block;
	return SD_MMC_OK;
}

sd_mmc_err_t sd_mmc_start_read_blocks(void *dest, uint16_t nb_block)
{
	if (!driver_start_read_blocks(dest, nb_block)) {
		sd_mmc_nb_block_remaining = 0;
		return SD_MMC_ERR_COMM;
	}
	sd_mmc_nb_block_remaining -= nb_block;
	return SD_MMC_OK;
}

sd_mmc_err_t sd_mmc_wait_end_of_read_blocks(void)
{
	if (!driver_wait_end_of_read_blocks()) {
		return SD_MMC_ERR_COMM;
	}
	if (sd_mmc_nb_block_remaining) {
		return SD_MMC_OK;
	}

	// All blocks are transfered then stop read operation
	if (sd_mmc_nb_block_to_tranfer == 1) {
		// Single block transfer, then nothing to do
		sd_mmc_deselect_slot();
		return SD_MMC_OK;
	}
	// WORKAROUND for no compliance card (Atmel Internal ref. !MMC7 !SD19):
	// The errors on this command must be ignored
	// and one retry can be necessary in SPI mode for no compliance card.
	if (!driver_adtc_stop(SDMMC_CMD12_STOP_TRANSMISSION, 0)) {
		driver_adtc_stop(SDMMC_CMD12_STOP_TRANSMISSION, 0);
	}
	sd_mmc_deselect_slot();
	return SD_MMC_OK;
}

sd_mmc_err_t sd_mmc_init_write_blocks(uint8_t slot, uint32_t start,
		uint16_t nb_block)
{
	sd_mmc_err_t sd_mmc_err;
	uint32_t cmd, arg, resp;

	sd_mmc_err = sd_mmc_select_slot(slot);
	if (sd_mmc_err != SD_MMC_OK) {
		return sd_mmc_err;
	}
	if (sd_mmc_is_write_protected(slot)) {
		sd_mmc_deselect_slot();
		return SD_MMC_ERR_WP;
	}

	if (nb_block > 1) {
		cmd = SDMMC_CMD25_WRITE_MULTIPLE_BLOCK;
	} else {
		cmd = SDMMC_CMD24_WRITE_BLOCK;
	}

	if (sd_mmc_card->type & CARD_TYPE_HC) {
		arg = start;
	} else {
		arg = (start * SD_MMC_BLOCK_SIZE);
	}
	if (!driver_adtc_start(cmd, arg, SD_MMC_BLOCK_SIZE, nb_block, true)) {
		sd_mmc_deselect_slot();
		return SD_MMC_ERR_COMM;
	}
	// Check response
	if (sd_mmc_is_mci()) {
		resp = driver_get_response();
		if (resp & CARD_STATUS_ERR_RD_WR) {
			sd_mmc_deselect_slot();
			return SD_MMC_ERR_COMM;
		}
	}
	sd_mmc_nb_block_remaining = nb_block;
	sd_mmc_nb_block_to_tranfer = nb_block;
	return SD_MMC_OK;
}

sd_mmc_err_t sd_mmc_start_write_blocks(const void *src, uint16_t nb_block)
{
	if (!driver_start_write_blocks(src, nb_block)) {
		sd_mmc_nb_block_remaining = 0;
		return SD_MMC_ERR_COMM;
	}
	sd_mmc_nb_block_remaining -= nb_block;
	return SD_MMC_OK;
}

sd_mmc_err_t sd_mmc_wait_end_of_write_blocks(void)
{
	if (!driver_wait_end_of_write_blocks()) {
		return SD_MMC_ERR_COMM;
	}
	if (sd_mmc_nb_block_remaining) {
		return SD_MMC_OK;
	}

	// All blocks are transfered then stop write operation
	if (sd_mmc_nb_block_to_tranfer == 1) {
		// Single block transfer, then nothing to do
		sd_mmc_deselect_slot();
		return SD_MMC_OK;
	}

	if (sd_mmc_is_mci()) {
		// Note: SPI multiblock writes terminate using a special
		// token, not a STOP_TRANSMISSION request.
		if (!driver_adtc_stop(SDMMC_CMD12_STOP_TRANSMISSION, 0)) {
			sd_mmc_deselect_slot();
			return SD_MMC_ERR_COMM;
		}
	}
	sd_mmc_deselect_slot();
	return SD_MMC_OK;
}

void sd_write_blk(uint8_t* buf, uint32_t addr)
{
    sd_mmc_err_t sd_mmc_err;
    
    do{
        sd_mmc_err = sd_mmc_init_write_blocks(0, addr, 1);
        if(sd_mmc_err != SD_MMC_OK)
        {
            sd_mmc_init();
        }
    }while(sd_mmc_err != SD_MMC_OK);
    
    do{
        sd_mmc_err = sd_mmc_start_write_blocks(buf, 1);
    }while(sd_mmc_err != SD_MMC_OK);
    
    do{
        sd_mmc_err = sd_mmc_wait_end_of_write_blocks();
    }while(sd_mmc_err != SD_MMC_OK);
}

void sd_read_blk(uint8_t* buf, uint32_t addr)
{
    sd_mmc_err_t sd_mmc_err;
    
    do{
        sd_mmc_err = sd_mmc_init_read_blocks(0, addr, 1);
        if(sd_mmc_err != SD_MMC_OK)
        {
            sd_mmc_init();
        }
    }while(sd_mmc_err != SD_MMC_OK);
   
    
    //led_set_status(LED_2, LED_ON);
    
    do{
        sd_mmc_err = sd_mmc_start_read_blocks(buf, 1);                            
    }while(sd_mmc_err != SD_MMC_OK);
    
    //led_set_status(LED_3, LED_ON);
    
    do{
        sd_mmc_err = sd_mmc_wait_end_of_read_blocks();
    }while(sd_mmc_err != SD_MMC_OK);
    
    //led_set_status(LED_4, LED_ON);
}

#ifdef SDIO_SUPPORT_ENABLE
sd_mmc_err_t sdio_read_direct(uint8_t slot, uint8_t func_num, uint32_t addr,
		uint8_t *dest)
{
	sd_mmc_err_t sd_mmc_err;

	if (dest == NULL) {
		return SD_MMC_ERR_PARAM;
	}

	sd_mmc_err = sd_mmc_select_slot(slot);
	if (sd_mmc_err != SD_MMC_OK) {
		return sd_mmc_err;
	}

	if (!sdio_cmd52(SDIO_CMD52_READ_FLAG, func_num, addr, 0, dest)) {
		sd_mmc_deselect_slot();
		return SD_MMC_ERR_COMM;
	}
	sd_mmc_deselect_slot();
	return SD_MMC_OK;
}

sd_mmc_err_t sdio_write_direct(uint8_t slot, uint8_t func_num, uint32_t addr,
		uint8_t data)
{
	sd_mmc_err_t sd_mmc_err;

	sd_mmc_err = sd_mmc_select_slot(slot);
	if (sd_mmc_err != SD_MMC_OK) {
		return sd_mmc_err;
	}

	if (!sdio_cmd52(SDIO_CMD52_WRITE_FLAG, func_num, addr, 0, &data)) {
		sd_mmc_deselect_slot();
		return SD_MMC_ERR_COMM;
	}

	sd_mmc_deselect_slot();
	return SD_MMC_OK;
}

sd_mmc_err_t sdio_read_extended(uint8_t slot, uint8_t func_num, uint32_t addr,
		uint8_t inc_addr, uint8_t *dest, uint16_t size)
{
	sd_mmc_err_t sd_mmc_err;

	if ((size == 0) || (size > 512)) {
		return SD_MMC_ERR_PARAM;
	}

	sd_mmc_err = sd_mmc_select_slot(slot);
	if (sd_mmc_err != SD_MMC_OK) {
		return sd_mmc_err;
	}

	if (!sdio_cmd53(SDIO_CMD53_READ_FLAG, func_num, addr, inc_addr,
			size, true)) {
		sd_mmc_deselect_slot();
		return SD_MMC_ERR_COMM;
	}
	if (!driver_start_read_blocks(dest, 1)) {
		sd_mmc_deselect_slot();
		return SD_MMC_ERR_COMM;
	}
	if (!driver_wait_end_of_read_blocks()) {
		sd_mmc_deselect_slot();
		return SD_MMC_ERR_COMM;
	}

	sd_mmc_deselect_slot();
	return SD_MMC_OK;
}

sd_mmc_err_t sdio_write_extended(uint8_t slot, uint8_t func_num, uint32_t addr,
		uint8_t inc_addr, uint8_t *src, uint16_t size)
{
	sd_mmc_err_t sd_mmc_err;

	if ((size == 0) || (size > 512)) {
		return SD_MMC_ERR_PARAM;
	}

	sd_mmc_err = sd_mmc_select_slot(slot);
	if (sd_mmc_err != SD_MMC_OK) {
		return sd_mmc_err;
	}

	if (!sdio_cmd53(SDIO_CMD53_WRITE_FLAG, func_num, addr, inc_addr,
			size, true)) {
		sd_mmc_deselect_slot();
		return SD_MMC_ERR_COMM;
	}
	if (!driver_start_write_blocks(src, 1)) {
		sd_mmc_deselect_slot();
		return SD_MMC_ERR_COMM;
	}
	if (!driver_wait_end_of_write_blocks()) {
		sd_mmc_deselect_slot();
		return SD_MMC_ERR_COMM;
	}

	sd_mmc_deselect_slot();
	return SD_MMC_OK;
}


                                     
#endif // SDIO_SUPPORT_ENABLE

